#include <chrono>
#include <memory>
#include <sys/time.h>
#include <unistd.h>
#include <sys/types.h>
#include <errno.h>
#include <sys/syscall.h>
#include <mutex>
#include <coroutine>
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/executor.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_msgs/msg/string.hpp"
#include "cospike/coroutine.hpp"

#include "cuda_runtime.h"
#include "device_types.h"
#include "exp/exp0_cuda.cuh"

using namespace std::chrono_literals;
using std::placeholders::_1;
#define gettid() syscall(__NR_gettid)
#define USE_INTRA_PROCESS_COMMS true 
#define DUMMY_LOAD_ITER	1000
#define THREAD_SIZE     3

timeval starting_time;
int dummy_load_calib = 1;

void dummy_load(int load_ms, const char * name_str) {
    int i, j;
    for (j = 0; j < dummy_load_calib * load_ms; j++)
        for (i = 0 ; i < DUMMY_LOAD_ITER; i++) 
            __asm__ volatile ("nop");
    rclcpp::sleep_for(100ms);
}

int dummy_load_sleep(int load_ms, const char * name_str) {
    int i, j;
    timeval ftime, ctime;
    // Do sth.
    for (j = 0; j < dummy_load_calib * load_ms; j++)
        for (i = 0 ; i < DUMMY_LOAD_ITER; i++) 
            __asm__ volatile ("nop");
    
    gettimeofday(&ftime, NULL);
    int duration_us = (ftime.tv_sec - starting_time.tv_sec) * 1000000 + (ftime.tv_usec - starting_time.tv_usec);
    long tv_sec = duration_us / 1000000;
    long tv_usec = duration_us - tv_sec * 1000000;

    // Wait for the machine
    rclcpp::sleep_for(450ms);

    gettimeofday(&ctime, NULL);
    duration_us = (ctime.tv_sec - starting_time.tv_sec) * 1000000 + (ctime.tv_usec - starting_time.tv_usec);
    tv_sec = duration_us / 1000000;
    tv_usec = duration_us - tv_sec * 1000000;
    
    // Do sth. Further
    for (j = 0; j < dummy_load_calib * load_ms; j++)
        for (i = 0 ; i < DUMMY_LOAD_ITER; i++) 
            __asm__ volatile ("nop");
    return 1;
}

namespace cb_chain_demo
{
class StartNode : public rclcpp::Node
{
public:
    StartNode(const std::string node_name, const std::string pub_topic, int period, bool use_co) 
        : Node(node_name, rclcpp::NodeOptions().use_intra_process_comms(USE_INTRA_PROCESS_COMMS)), count_(0), period_(period), use_co_(use_co)
    {
        publisher_ = this->create_publisher<std_msgs::msg::String>(pub_topic, 1);
        name_ = node_name;
        timer_ = this->create_wall_timer(std::chrono::duration<int, std::chrono::milliseconds::period>(this->period_), std::bind(&StartNode::timer_callback, this));
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
private:
    std::string name_;
    size_t count_;
    int period_;
    timeval ctime, ftime, create_timer, latency_time;
    bool use_co_;

    void show_time(timeval ftime, timeval ctime) 
    {
        int duration_us = (ftime.tv_sec - starting_time.tv_sec) * 1000000 + (ftime.tv_usec - starting_time.tv_usec);
        long tv_sec = duration_us / 1000000;
        long tv_usec = duration_us - tv_sec * 1000000;
        RCLCPP_INFO(this->get_logger(), "[PID: %ld] [Bgn] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);

        duration_us = (ctime.tv_sec - starting_time.tv_sec) * 1000000 + (ctime.tv_usec - starting_time.tv_usec);
        tv_sec = duration_us / 1000000;
        tv_usec = duration_us - tv_sec * 1000000;
        RCLCPP_INFO(this->get_logger(), "[PID: %ld] [End] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);
    }

    void timer_callback()
    {
        gettimeofday(&ftime, NULL);
        dummy_load(100, this->name_.c_str());
        std::string name = this->get_name();
        auto message = std_msgs::msg::String();
        message.data = std::to_string(count_++);
        gettimeofday(&ctime, NULL);
        publisher_->publish(message);
        show_time(ftime, ctime);
    }
};

class IntermediateNodeCPU : public rclcpp::Node
{
public:
    IntermediateNodeCPU(const std::string node_name, const std::string sub_topic, const std::string pub_topic, int exe_time, bool use_co) 
        : Node(node_name), count_(0), exe_time_(exe_time), use_co_(use_co)
    {                        
        // create_subscription interface for sync callback
        subscription_ = this->create_subscription<std_msgs::msg::String>(false, sub_topic, 1, std::bind(&IntermediateNodeCPU::callback, this, _1));
        if (pub_topic != "") publisher_ = this->create_publisher<std_msgs::msg::String>(pub_topic, 1);
        this->name_ = node_name;
    }
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;

private:
    std::string name_;
    size_t count_;
    int exe_time_;
    timeval ctime, ftime;
    double latency;
    bool use_co_;

    void show_time(timeval ftime, timeval ctime, int mark = false) 
    {
        int duration_us = (ftime.tv_sec - starting_time.tv_sec) * 1000000 + (ftime.tv_usec - starting_time.tv_usec);
        long tv_sec = duration_us / 1000000;
        long tv_usec = duration_us - tv_sec * 1000000;
        if (mark) RCLCPP_INFO(this->get_logger(), "[*] [PID: %ld] [Bgn] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);
        else RCLCPP_INFO(this->get_logger(), "[PID: %ld] [Bgn] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);

        duration_us = (ctime.tv_sec - starting_time.tv_sec) * 1000000 + (ctime.tv_usec - starting_time.tv_usec);
        tv_sec = duration_us / 1000000;
        tv_usec = duration_us - tv_sec * 1000000;
        if (mark) RCLCPP_INFO(this->get_logger(), "[*] [PID: %ld] [End] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);
        else RCLCPP_INFO(this->get_logger(), "[PID: %ld] [End] [s: %ld] [us: %ld]", gettid(), tv_sec, tv_usec);
    }

    int callback(const std_msgs::msg::String::SharedPtr msg) {
        gettimeofday(&ftime, NULL);
        gettimeofday(&ctime, NULL);
        show_time(ftime, ctime);
        /* Non-Blocking Style */
        rclcpp::sleep_for(2000ms);
        gettimeofday(&ftime, NULL);
        std::string name = this->get_name();
        auto message = std_msgs::msg::String();
        message.data = msg->data;
        gettimeofday(&ctime, NULL);
        if (publisher_) publisher_->publish(message);
        show_time(ftime, ctime, true);
        return 1;
    }
};


}

void run_exe(rclcpp::executors::ExecutorNodelet* exe) {
    exe->spin();
}

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);

    bool co_en_4_exp = true;
    int exetension_rate = 10;

    // Create executors
    int number_of_node = std::thread::hardware_concurrency() * 2.5;
    rclcpp::executors::MultiThreadedExecutor exec1(rclcpp::executor::ExecutorArgs(), std::thread::hardware_concurrency() / 3, true);
    printf("Thread = %d, Node = %d + 1\n", exec1.get_number_of_threads(), number_of_node);
    std::queue<Task<int, RosCoExecutor> > task_queue;

    // Define graph --> 1T -> 2N
    auto c1_timer = std::make_shared<cb_chain_demo::StartNode>("Timer_callback1", "c1", 800*exetension_rate,  false);
    exec1.add_node(c1_timer);

    std::vector<std::shared_ptr<cb_chain_demo::IntermediateNodeCPU> > node_list;

    for (int i = 0; i < number_of_node; i++) {
        std::string name = "Regular_callback_" + std::to_string(i);
        auto cb_node = std::make_shared<cb_chain_demo::IntermediateNodeCPU>(name.c_str(), "c1", "", 100, false);
        exec1.add_node(cb_node);
        node_list.push_back(cb_node);
        printf("Insert Node %d\n", i);
    }

    // Execution
    gettimeofday(&starting_time, NULL);
    exec1.spin();

    // Remove Extra-node
    exec1.remove_node(c1_timer);
    for (int i = 0; i < number_of_node; i++) {
        exec1.remove_node(node_list[i]);
    }

    // Shutdown
    rclcpp::shutdown();
    return 0;
}